#!/usr/bin/env python

import carla
import numpy as np
import time
import random
from queue import Queue, Empty
import cv2
import os
import copy
import logging
from carla import Transform, Rotation, Location
import shutil
import stat
import json
from carla import Map
from carla import Vector3D
import math
from carla import ColorConverter as cc

def points2pcd(PCD_FILE_PATH, points):
    # 存放路径
    # PCD_DIR_PATH = os.path.join(os.path.abspath('.'), 'pcd')
    # PCD_FILE_PATH = os.path.join(PCD_DIR_PATH, 'cache.pcd')
    if os.path.exists(PCD_FILE_PATH):
        os.remove(PCD_FILE_PATH)
    # 写文件句柄
    handle = open(PCD_FILE_PATH, 'a')
    # 得到点云点数
    point_num = points.shape[0]
    # pcd头部（重要）
    handle.write(
        '# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z intensity\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1')
    string = '\nWIDTH ' + str(point_num)
    handle.write(string)
    handle.write('\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0')
    string = '\nPOINTS ' + str(point_num)
    handle.write(string)
    handle.write('\nDATA ascii')

    # 依次写入点
    for i in range(point_num):
        string = '\n' + str(points[i, 0]) + ' ' + str(points[i, 1]) + ' ' + str(points[i, 2]) + ' ' + str(points[i, 3])
        handle.write(string)
    handle.close()

def get_time_stamp(ct):
    """
    :param ct: float时间
    :return: 带毫秒的格式化时间戳
    """
    # ct = time.time()
    # print(ct)
    local_time = time.localtime(ct)
    # data_head = time.strftime("%Y-%m-%d %H:%M:%S", local_time)
    data_head = time.strftime("%Y-%m-%d %H-%M-%S", local_time).split(' ')[-1]
    data_secs = (ct - int(ct)) * 1000
    time_stamp = "%s-%03d" % (data_head, data_secs)
    # print(time_stamp, type(time_stamp))
    # stamp = ("".join(time_stamp.split()[0].split("-"))+"".join(time_stamp.split()[1].split(":"))).replace('.', '')
    # print(stamp)
    return time_stamp

def sensor_callback(sensor_data, sensor_queue, sensor_name):
    # Do stuff with the sensor_data data like save it to disk
    # Then you just need to add to the queue
    sensor_queue.put((sensor_data.frame, sensor_data.timestamp, sensor_name, sensor_data))

# modify from leaderboard
def _parse_lidar_cb(lidar_data):
    points = np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4'))
    points = copy.deepcopy(points)
    points = np.reshape(points, (int(points.shape[0] / 4), 4))
    return points

def _parse_radar_cb(radar_data):
    points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4'))
    points = copy.deepcopy(points)
    points = np.reshape(points, (len(radar_data), 4))
    return points

# modify from world on rail code
def lidar_to_bev(lidar, min_x=-24, max_x=24, min_y=-16, max_y=16, pixels_per_meter=4, hist_max_per_pixel=10):
    xbins = np.linspace(
        min_x, max_x + 1,
               (max_x - min_x) * pixels_per_meter + 1,
    )
    ybins = np.linspace(
        min_y, max_y + 1,
               (max_y - min_y) * pixels_per_meter + 1,
    )
    # Compute histogram of x and y coordinates of points.
    hist = np.histogramdd(lidar[..., :2], bins=(xbins, ybins))[0]
    # Clip histogram
    hist[hist > hist_max_per_pixel] = hist_max_per_pixel
    # Normalize histogram by the maximum number of points in a bin we care about.
    overhead_splat = hist / hist_max_per_pixel * 255.
    # Return splat in X x Y orientation, with X parallel to car axis, Y perp, both parallel to ground.
    return overhead_splat[::-1, :]


# # 可视化数据
# def visualize_data(rgb, lidars, text_args=(cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255, 255, 255), 1)):
#     rgb_canvas = np.array(rgb[..., ::-1])
#     # print(rgb_canvas.shape, rgb_canvas.size)
#     canvas_list = []
#     lidar_canvas = None
#     if lidars is not None:
#         for lidar in lidars:
#             lidar_viz = lidar_to_bev(lidar).astype(np.uint8)
#             lidar_viz = cv2.cvtColor(lidar_viz, cv2.COLOR_GRAY2RGB)
#             canvas = cv2.resize(lidar_viz.astype(np.uint8), (rgb_canvas.shape[0], rgb_canvas.shape[0]))
#             canvas_list.append(canvas)
#         lidar_canvas = np.concatenate(canvas_list, axis=1)
#     # cv2.putText(canvas, f'yaw angle: {imu_yaw:.3f}', (4, 10), *text_args)
#     # cv2.putText(canvas, f'log: {gnss[0]:.3f} alt: {gnss[1]:.3f} brake: {gnss[2]:.3f}', (4, 20), *text_args)
#     return lidar_canvas


# def mkdir_folder(path):
#     for s_type in sensor_type:
#         if not os.path.isdir(os.path.join(path, s_type)):
#             os.makedirs(os.path.join(path, s_type))
#     return True


def draw_waypoints(world, waypoints, road_id=None, life_time=50.0):
    """
    :param waypoints: 地图所有航点列表
    :param road_id: 目标路段id
    :param life_time: 高亮时间
    :return:
    """
    obj_waypoints = []

    for waypoint in waypoints:
        # if waypoint.road_id == road_id:
        if True:
            obj_waypoints.append(waypoint)
            world.debug.draw_string(waypoint.transform.location, 'O', draw_shadow=False,
                                    color=carla.Color(r=0, g=255, b=0), life_time=life_time,
                                    persistent_lines=True)
    return obj_waypoints

def vehicle_trans(x0, y0, yaw, l):
    """
    :param x0: Carla中车辆的x坐标
    :param y0: Carla中车辆的y坐标
    :param yaw: Carla中车辆的航向角(deg)
    :param l: 车辆几何中心到后轴中心的距离（米）
    :return:
    """
    #  同步到51simone中车辆的后轴中心坐标
    x = x0 + l * math.cos(yaw * math.pi / 180.0)
    y = y0 + l * math.sin(yaw * math.pi / 180.0)
    return x, y 

def main():
    #  ======================================跟服务器实现连接=============================================
    client = carla.Client("localhost", 2000)
    client.set_timeout(20.0)  # 设置这个客户端连接超时时间
    with open(word_path) as od_file:
        data = od_file.read()
    # 使用自定义地图
    world = client.generate_opendrive_world(data,
                                            carla.OpendriveGenerationParameters(
                                                wall_height=0))
    ## 加载carla自带地图，该地图列表可通过client.get_available_maps()查看
    ## print(client.get_available_maps())  # 获取可用的地图列表（自带的地图，无自定义地图）
    # world = client.load_world('Town02') 
    world = client.get_world()
    synchronous_master = False
    
    ##  显示车道中心点坐标
    # waypoints = world.get_map().generate_waypoints(0.2)
    # v_waypoints = []
    # for waypoint in waypoints:
    #     if waypoint.transform.location.x < 399 and waypoint.transform.location.x > 300:
    #         if waypoint.transform.location.y < -600 and waypoint.transform.location.y > -699:
    #             v_waypoints.append(waypoint)

    # obj_waypoints = draw_waypoints(world, v_waypoints, life_time=100)


    try:
        # =================================调整路网中的环境可见度======================================
        # # 使得道路中自动生成的交通灯为不可见状态
        # env_objs1 = world.get_environment_objects(carla.CityObjectLabel.TrafficLight)
        # env_objs2 = world.get_environment_objects(carla.CityObjectLabel.Poles)
    
        # env_list = []
        # for env_obj in env_objs1:
        #     env_list.append(env_obj.id)
        # for env_obj in env_objs2:
        #     env_list.append(env_obj.id)
        # world.enable_environment_objects(env_list, False)


        # ==================================新建pcd存储文件夹==========================================
        pt = world.get_snapshot().timestamp.platform_timestamp
        ptime = int(pt)
        if SAVE_PCD:
            # 新建pcd保存文件夹
            save_pcd_path = os.path.join(save_path, str(ptime))
            os.makedirs(save_pcd_path)

        if RECORDER:
            # 存储recorder记录文件
            client.start_recorder(recorder_path + str(ptime) + '.log', True)


        # ====================================获取世界视角=============================================
        spectator = world.get_spectator()
        world.debug.draw_string(vehicle_trans1.location, 'M', draw_shadow=False,
                                color=carla.Color(r=0, g=255, b=0), life_time=1500, persistent_lines=True)
        spectator.set_transform(vehicle_trans1)

        world.debug.draw_string(p.location, 'X', draw_shadow=False,
                                color=carla.Color(r=255, g=0, b=0), life_time=1500, persistent_lines=True)

    
        # ============================== 修改世界设置:更新步长和同步模式===================================
           
        # # 让车辆按照交通规则在世界中行驶
        #设置TM接入server的端口，默认8000
        traffic_manager = client.get_trafficmanager(8000)
        #TM里的每一辆车至少保持2米的安全距离
        traffic_manager.set_global_distance_to_leading_vehicle(2.0)
        #TM里每一辆车都是默认速度的40%（默认所有车辆限速30km/h）
        traffic_manager.global_percentage_speed_difference(40.0)
        if True:
            #设置TM同步模式
            settings = world.get_settings()
            traffic_manager.set_synchronous_mode(True)
            if not settings.synchronous_mode:
                print("开启同步模式")
                synchronous_master = True
                settings.synchronous_mode = True
                settings.fixed_delta_seconds = 0.1
                world.apply_settings(settings)
            else:
                synchronous_master = False

        # =============================天气=================================

        # # 控制世界的天气和时间（太阳的位置） 万里无云，没有降雨，太阳的角度为90
        weather = carla.WeatherParameters(
            cloudiness=0.0,  # 0-100  0 是晴朗的天空，100 是完全阴天
            precipitation=0.0,  # 0 表示没有下雨，100 表示大雨
            precipitation_deposits=0.0, # 0 表示道路上没有水坑，100 表示道路完全被雨水覆盖
            wind_intensity=0.0, # 0 表示平静，100 表示强风，风会影响雨向和树叶
            sun_azimuth_angle=0.0, # 太阳方位角，0～360
            sun_altitude_angle=90.0,  # 太阳高度角，90 是中午，-90 是午夜
            fog_density=0.0, # 0～100 表示雾的浓度或厚度，仅影响RGB相机传感器
            fog_distance=0.0, # 雾开始的距离，单位为米
            wetness=0.0, # 0～100 表示道路湿度百分比，仅影响RGB相机传感器
            fog_falloff=0.0, # 雾的密度，0至无穷大，0 表示雾比空气轻，覆盖整个场景，1表示与空气一样，覆盖正常大小的建筑物
            scattering_intensity=0.0, # 控制光线对雾的穿透程度
            mie_scattering_scale=0.0, # 控制光线与花粉或空气等大颗粒的相互作用，导致天气朦胧，光源周围有光晕，0表示无影响
            rayleigh_scattering_scale=0.0331, # 控制光与空气分子等小粒子的相互作用，取决于光波长，导致白天蓝天或晚上红天
            )
        world.set_weather(weather)

        # ======================通过蓝图库模糊搜索指定的车辆模型===============
        blueprint_library = world.get_blueprint_library()

        # =============================车辆=================================

        # wbp = world.get_blueprint_library().find('walker.pedestrian.0004')
        # wbp.set_attribute('speed', '0.0')
        # walker = world.spawn_actor(wbp, walker_trans)
        # actor_list.append(walker)

                #添加车辆
        bp0 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        # bp1 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        # bp2 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        # bp3 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        # bp4 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        # bp5 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        # bp6 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        # bp7 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        # bp8 = blueprint_library.find('vehicle.lincoln.mkz_2017')

        bp0.set_attribute('color', '0, 0, 0')
        # bp1.set_attribute('color', '0, 0, 0')
        # bp2.set_attribute('color', '0, 0, 0')
        # bp3.set_attribute('color', '0, 0, 0')
        # bp4.set_attribute('color', '0, 0, 0')
        # bp5.set_attribute('color', '0, 0, 0')
        # bp6.set_attribute('color', '0, 0, 0')
        # bp7.set_attribute('color', '0, 0, 0')
        # bp8.set_attribute('color', '0, 0, 0')

        ego_vehicle0 = world.spawn_actor(bp0, vehicle_trans0)
        ego_vehicle1 = world.spawn_actor(bp0, vehicle_trans1)
        ego_vehicle2 = world.spawn_actor(bp0, vehicle_trans2)
        ego_vehicle3 = world.spawn_actor(bp0, vehicle_trans3)
        ego_vehicle4 = world.spawn_actor(bp0, vehicle_trans4)
        ego_vehicle5 = world.spawn_actor(bp0, vehicle_trans5)
        ego_vehicle6 = world.spawn_actor(bp0, vehicle_trans6)
        ego_vehicle7 = world.spawn_actor(bp0, vehicle_trans7)
        ego_vehicle8 = world.spawn_actor(bp0, vehicle_trans8)
        print("单独放置车辆")
        actor_list.append(ego_vehicle0)
        actor_list.append(ego_vehicle1)
        actor_list.append(ego_vehicle2)
        actor_list.append(ego_vehicle3)
        actor_list.append(ego_vehicle4)
        actor_list.append(ego_vehicle5)
        actor_list.append(ego_vehicle6)
        actor_list.append(ego_vehicle7)
        actor_list.append(ego_vehicle8)

        # #获取车辆boundingbox的长宽高信息
        # length1 = ego_vehicle1.bounding_box.extent.x * 2.0
        # width1 = ego_vehicle1.bounding_box.extent.y * 2.0
        # higth1 = ego_vehicle1.bounding_box.extent.z * 2.0
        # length2 = ego_vehicle2.bounding_box.extent.x * 2.0
        # width2 = ego_vehicle2.bounding_box.extent.y * 2.0
        # higth2 = ego_vehicle2.bounding_box.extent.z * 2.0
        # print("\n1: l:{0}, w:{1}, h:{2}, 2: l:{3}, w:{4}, h:{5}".format(
        #     length1, width1, higth1, length2, width2, higth2))

        #车辆控制
        # ego_vehicle1.set_autopilot(True)
        ego_vehicle0.apply_control(carla.VehicleControl(hand_brake = True))
        ego_vehicle1.apply_control(carla.VehicleControl(hand_brake = True))
        ego_vehicle2.apply_control(carla.VehicleControl(hand_brake = True))
        ego_vehicle3.apply_control(carla.VehicleControl(hand_brake = True))
        ego_vehicle4.apply_control(carla.VehicleControl(hand_brake = True))
        ego_vehicle5.apply_control(carla.VehicleControl(hand_brake = True))
        ego_vehicle6.apply_control(carla.VehicleControl(hand_brake = True))
        ego_vehicle7.apply_control(carla.VehicleControl(hand_brake = True))
        ego_vehicle8.apply_control(carla.VehicleControl(hand_brake = True))
       

        # ===========================实例化传感器模型============================
        # =======================================传感器==================================

        lidar_bp_64 = blueprint_library.find('sensor.lidar.ray_cast')

        # 设置传感器的参数
        atmosphere_attenuation_rate = '0.008'
        dropoff_general_rate = '0.0'
        dropoff_intensity_limit = '0.0'
        dropoff_zero_intensity = '0.0'
        noise_seed = '0.0'
        noise_stddev = '0.0'

        lidar_bp_64.set_attribute('channels', '64')
        lidar_bp_64.set_attribute('upper_fov', '+2.0')
        lidar_bp_64.set_attribute('lower_fov', '-24.8')
        lidar_bp_64.set_attribute('points_per_second', '1333000')
        lidar_bp_64.set_attribute('range', '120')
        lidar_bp_64.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))
        lidar_bp_64.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
        lidar_bp_64.set_attribute('dropoff_general_rate', dropoff_general_rate)
        lidar_bp_64.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
        lidar_bp_64.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
        lidar_bp_64.set_attribute('noise_seed', noise_seed)
        lidar_bp_64.set_attribute('noise_stddev', noise_stddev)

        lidar_64 = world.spawn_actor(lidar_bp_64, lidar_trans, attach_to=ego_vehicle0)
        lidar_64.listen(lambda data: sensor_callback(data, sensor_queue, "lidar_64"))

        sensor_list.append(lidar_64)

        while True:
            # print(sensor_queue.qsize())
            world.tick()
            w_frame = world.get_snapshot().frame
            p_timestamp = world.get_snapshot().timestamp.platform_timestamp
            w_timestamp = get_time_stamp(p_timestamp)
            print("\nWorld's frame:{0}, time: {1}".format(w_frame, w_timestamp))

            try:
                lidars = []
                
                #传感器数据保存
                for i in range(0, len(sensor_list)):
                    s_frame, s_timestamp, s_name, s_data = sensor_queue.get(True, 1.0)
                        
                    lidars.append(_parse_lidar_cb(s_data))

                if lidars and SAVE_PCD:
                    concat_points = np.concatenate(lidars, axis=0)
                    concat_points[:, 1] = [-p for p in concat_points[:, 1]] #  将生成的点云Y值进行反转 由于carla的坐标系中Y值是反的
                    pcd_path = save_pcd_path + "/" + str(w_timestamp) + ".pcd"
                    points2pcd(pcd_path, concat_points)
                
                # 传感器数据可视化
                # rgb = np.concatenate(rgbs, axis=1)[..., :3]
                # lidar32 = visualize_data(rgb, lidars)
                # lidarsplice = visualize_data(rgb, [concat_points1])
                # cv2.imshow('rgb_vizs', rgb)
                # cv2.imshow('lidar_32', lidar32)
                # cv2.imshow('lidar_splice', lidarsplice)
                # cv2.waitKey(1)

            except Empty:
                print("[Warning] Some of the sensor information is missed")
            time.sleep(0.1)

    finally:
        if synchronous_master:
            settings = world.get_settings()
            settings.synchronous_mode = False
            settings.fixed_delta_seconds = None
            world.apply_settings(settings)

        for sensor in sensor_list:
            sensor.destroy()
        for actor in actor_list:
            if actor.is_alive:
                actor.destroy()
        print("All cleaned up!")
        if RECORDER:
            #关闭recorder
            client.stop_recorder()

if __name__ == "__main__":
    word_path = r"/home/wanji/下载/carla_test/net_files/000107.xodr"
    sensor_type = ['rgb', 'lidar']
    sensor_queue = Queue()
    actor_list, sensor_list = [], []
    save_path = '/home/wanji/下载/carla_test/output/'
    recorder_path = '/home/wanji/carla/recorder_files/'
    # realdata = '/home/wanji/下载/carla_test/output/toveiw.json'
    # if os.path.exists(save_path):
    #     for fileList in os.walk(save_path):
    #         for name in fileList[2]:
    #             os.chmod(os.path.join(fileList[0], name), stat.S_IWRITE)
    #             os.remove(os.path.join(fileList[0], name))
    #     shutil.rmtree(save_path)

    # os.mkdir(save_path)
    IM_WIDTH = 256 * 1
    IM_HEIGHT = 256 * 1

    RECORDER = False  #  是否需要回放
    SAVE_PCD = True  #  是否需要保存lidar点云的pcd格式数据

    view_transform = Transform(Location(x=371.9938132585651, y=-643.7199381087686, z=43.017), Rotation(pitch=-0.1249, yaw=200.3, roll=-0.2931))

    # 与51simone相比，航向角yaw_carla =  360 - yaw_51   pitch=-0.1249, yaw=260.3, roll=-0.3431
    lidar_trans = Transform(Location(x=0.0, y=0.0, z=1.73), 
                            Rotation(pitch=0.0, yaw=180.0, roll=0.0))


    # 与51simone相比，航向角yaw_carla = 360 - yaw_51
    vehicle_trans0 = Transform(Location(x=-32.5, y= 23.5, z=1.0), 
                               Rotation(pitch=0.000000, yaw = 179.2, roll=0.000000))
    vehicle_trans1 = Transform(Location(x=-27.69, y= 26.04, z=1.0), 
                               Rotation(pitch=0.000000, yaw = 0, roll=0.000000))
    vehicle_trans2 = Transform(Location(x=-22.21, y=26.23, z=1.0), 
                               Rotation(pitch=0.000000, yaw = 0, roll=0.000000))
    vehicle_trans3 = Transform(Location(x=-16.26, y=26.10, z=1.0), 
                               Rotation(pitch=0.000000, yaw = 0, roll=0.000000))
    vehicle_trans4 = Transform(Location(x=-9.11, y=19.7, z=1.0), 
                               Rotation(pitch=0.000000, yaw = -0.2, roll=0.000000))
    vehicle_trans5 = Transform(Location(x=-8.14, y=26.44, z=1.0), 
                               Rotation(pitch=0.000000, yaw = 0, roll=0.000000))
    vehicle_trans6 = Transform(Location(x=-0.17, y=26.13, z=1.0), 
                               Rotation(pitch=0.000000, yaw = 0, roll=0.000000))
    vehicle_trans7 = Transform(Location(x=10.96, y=25.86, z=1.0), 
                               Rotation(pitch=0.000000, yaw = 0, roll=0.000000))
    vehicle_trans8 = Transform(Location(x=17.90, y=25.91, z=1.0), 
                               Rotation(pitch=0.000000, yaw = 0, roll=0.000000))
    
    p = Transform(Location(x=371.9938132585651, y=-643.7199381087686, z=46), 
                               Rotation(pitch=-45.000000, yaw=0.0, roll=0.000000))

    try:
        main()
    except KeyboardInterrupt:
        print(' - Exited by user.')
